#include "IMU_ADIS16505.h"
#include "delay.h"
#include "SpiPeripheral.h"
#include "spi.h"
#include "usart.h"
#include "encoder.h"
#include "usart3.h"
#include "math.h"
#include "timer.h"
#include "kalman.h"
#include "filter.h"
#include <stdbool.h>


static  SpiPeripheral spi3(SPI3);
#define STATIC_DRIFT_CNT 100
extern  u8  Integr_Flag;  //定时器10ms中断标志

/**
* @Axis Z(Yaw), Y(Roll), X(Pitch)
*       Z
*       ^
*       |
*       |    Y
*       |   /
*       |  /
*       | /
*       |/
*       O-------------->X
*/

Imu_Adis16505::Imu_Adis16505(SpiPeripheral *trans)
{
    AccCoefficient        = 2.45f;
    AngleVelCoefficient   = 0.00625f;
    DeltaAngleCoefficient = 0.010986328125f;
    DeltaVelCoefficient   = 0.003052;
//    Calculus_Time         = 0.01  ;  //积分时间为10ms
    Calculus_Time         = 0.02  ;  //积分时间为20ms，效果最好  
//    Calculus_Time           = 0.015  ;  //积分时间为15ms     
//     Calculus_Time           = 0.005  ;  //积分时间为15ms
    AngleVel_32Bit_Coefficient   =0.000000095367431640625f;     //32位角速度系数
    Acc_32Bit_Coefficient        = 0.000037384033203125f;    //32位加速度系数
    TempCoefficient              =0.1;                              //温度系数
    
    DriftDataMax= 0.000833333;                         //设置静态偏移量的阈值最大值
    DriftDataMin=-0.000833333;                         //设置静态偏移量的阈值最小值
    
    Movaverage.mv_x.max_cnt=6;            //移动平均滤波队列长度
    Movaverage.mv_x.average=Movaverage.MfDataX;
    Movaverage.mv_y.max_cnt=6;            //移动平均滤波队列长度
    Movaverage.mv_y.average=Movaverage.MfDataY;
    Movaverage.mv_z.max_cnt=6;            //移动平均滤波队列长度
    Movaverage.mv_z.average=Movaverage.MfDataZ;
    
    for (u8 i = 0; i < 10; i++)
        this->Adis16505_Data.original_.Burst16_Data[i] = 0;
    this->Adis16505_Data.ZeroState.Zero_Order_Cnt = 0;
    for (u8 i = 0; i < 2; i++)
        this->Adis16505_Data.ZeroState.Zero_Flag[i] = 0x00;

    _ptrans = trans;

}

Imu_Adis16505 *Imu_Adis16505::get(void)
{
    static Imu_Adis16505 adis16505_(&spi3);
    return &adis16505_;
}

u16 Imu_Adis16505::readRegister(u16 regaddr)
{   //1661   13
    u16 ret[2] = {0, 0};
    u16 data = 0x0000;
    _ptrans->chipSelect();
    delay_us(9);
    ret[0] = _ptrans->readAfterWrite((uint16_t)((regaddr << 8) & 0x7F00));
    delay_us(8);  //
    //  ret[0]= _ptrans->readAfterWrite(0x7200);
    _ptrans->chipRelease();
 //     delay_us(17);
 //     delay_us(20);
 //   delay_us(13);
     delay_us(12);
    _ptrans->chipSelect();
    delay_us(9);   //
    ret[1] = _ptrans->readAfterWrite(0x0000);
    delay_us(8);
    _ptrans->chipRelease();
    data = ret[1];
    ret[1] = 0;
    ret[0] = 0;
    //  data=(ret[0]<<8)|ret[1];
    return data;
}

u16 Imu_Adis16505::writeRegister(uint16_t pBuffer, u16 RegisterAddr)
{
    u16 data[2] = {0, 0};
    data[0] = pBuffer & 0x00ff;
    data[1] = (pBuffer >> 8) & 0x00ff;
    u16 ret = 0;
    for (u8 i = 0; i < 2; i++)
    {
        _ptrans->chipSelect();
        delay_us(6);
        _ptrans->readAfterWrite((((RegisterAddr + i) << 8) | 0x8000) | data[i]);
        delay_us(1);
        _ptrans->chipRelease();
        delay_us(10);
    }
    return ret;
} 

void Imu_Adis16505 :: ImuResestInit()
{
   GPIO_InitTypeDef  GPIO_InitStructure;
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD, ENABLE);   //使能PD端口时钟
  
   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;         //LED0-->PB.5 端口配置
   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      //推挽输出
   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;     //IO口速度为50MHz
   GPIO_Init(GPIOD, &GPIO_InitStructure);           //根据设定参数初始化GPIOB.5
   GPIO_SetBits(GPIOD,GPIO_Pin_6);             //PB.5 输出高
}

void Imu_Adis16505 :: ImuResest()
{
   GPIO_SetBits(GPIOD,GPIO_Pin_6);             //PB.5 输出高
   delay_ms(5); 
   GPIO_ResetBits(GPIOD,GPIO_Pin_6);             //PB.5 输出高
   delay_ms(5);
   GPIO_SetBits(GPIOD,GPIO_Pin_6);               //PB.5 输出低
}


u16 Imu_Adis16505::getDeviceId(void)
{
    u16 ret = 0;
    delay_ms(1);
    ret = readRegister(ADIS16505_PROD_ID);
    Adis16505_Data.Imu_Id = ret;
    //      this->writeRegister(ADIS16505_BIT32_ACC, ADIS16505_MSC_CTRL); //配置为32位读取加速度模式
    //    delay_us(400);
    //    ret = readRegister(ADIS16505_MSC_CTRL);
    return ret;
}


bool Imu_Adis16505::init(void)
{
//    ImuResestInit();
//    ImuResest();
    
    _ptrans->begin();
    
    while (getDeviceId() != device_id)
    {
        delay_ms(1);
    }
    while(this->Adis16505SetReadMode(ADIS16505_BIT32_ACC) != Imu_Adis16505_STATUS_OK)
    {
        delay_ms(1);
    }
           
    while(ImuCalibration(0x1f4)!=Imu_Adis16505_STATUS_OK)          //Imu校准数据，获得静态漂移量      2000可以
    {
       delay_ms(1);
    }    
    return true;
}




Imu_Adis16505StatusTypeDef Imu_Adis16505 ::  ImuGet_Drift( int* Bit32_Drift)   //Imu获取数据静态漂移量
{
      if(Drinft_1S_flag==1)
      {
          if(Drinft_1S_cnt<=100)
          {
             Drinft_1S_cnt++;
             return  Imu_Adis16505_STATUS_ERROR; 
          }
          else if((Drinft_1S_cnt>50) &&(Drinft_1S_cnt<=150)) 
          {              
             Adis16505_Data.Drift_Data.Bit32_Mode.Drinft+= Bit32_Drift[3]   ;
             
             Drinft_1S_cnt++; 
             return  Imu_Adis16505_STATUS_ERROR; 
          }
          else if(Drinft_1S_cnt==151)
          {
            Adis16505_Data.Drift_Data.Bit32_Mode.Drinft= -( Adis16505_Data.Drift_Data.Bit32_Mode.Drinft/100.0f); 
            Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data= Adis16505_Data.Drift_Data.Bit32_Mode.Drinft*AngleVel_32Bit_Coefficient ;
            Drinft_1S_cnt=153; 
            return   Imu_Adis16505_STATUS_OK;
          }
          else
             return   Imu_Adis16505_STATUS_OK; 
      }
      else
          return  Imu_Adis16505_STATUS_ERROR;
}



float Imu_Adis16505 :: ImuUpdateDrift()
{ 
  if(DriftTask.task_flag== true)                     //判断wingbot是否静止
  {      
      if(ImuFlag.calibration==true)  
      { 
          int av1=0;
      
          if(DriftTask.time_flag==true)
           {
              if(Movaverage.mv_z.anglevel>=Adis16505_Data.wingbotDrift.max)
              {
                  Adis16505_Data.wingbotDrift.max=Movaverage.mv_z.anglevel;          //拿到一个循环中的最大极值
              }
              if(Movaverage.mv_z.anglevel<=Adis16505_Data.wingbotDrift.min)
              {
                Adis16505_Data.wingbotDrift.min=Movaverage.mv_z.anglevel;            //拿到一个循环中的最小极值
              }
              
              Adis16505_Data.wingbotDrift.filteranglevel= Movaverage.mv_z.anglevel;
              Adis16505_Data.wingbotDrift.sum+=Movaverage.mv_z.anglevel;     //15ms中断; 
              Adis16505_Data.wingbotDrift.sum_cnt++;                 //偏移累加量+1
            //  DriftTask.task_sum_cnt++;                              //进入中断次数+1
              if(Adis16505_Data.wingbotDrift.sum_cnt==1000)           //如果15s钟时间到，15ms中断，进入1000次为15s
              {
                //去掉极大，极小值  
                Adis16505_Data.wingbotDrift.sum=Adis16505_Data.wingbotDrift.sum- Adis16505_Data.wingbotDrift.max-Adis16505_Data.wingbotDrift.min;
                //计算去掉极值后998个点的均值作为偏移量
                Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=-(Adis16505_Data.wingbotDrift.sum/(1000.0f-2.0f));     //更新一次偏移量
                  //把当前偏移量保存到上次  
                Adis16505_Data.Drift_Data.Bit32_Mode.PreviousTrueDrift=Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift;
                
                Adis16505_Data.wingbotDrift.max =0;           //清空极大值
                Adis16505_Data.wingbotDrift.min =0;           //清空极小值
                  
                Adis16505_Data.wingbotDrift.sum_cnt=0;
                Adis16505_Data.wingbotDrift.sum=0;  
                DriftTask.task_sum_cnt=0;                          
              }
              else                                                    //如果1s钟时间未到
              {
                  if(DriftTask.task_flag== false)                     //判断wingbot是否静止
                  { 
                    //如果开始运动，则结算当前时间偏移量
                   // Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=-Adis16505_Data.wingbotDrift.sum/(Adis16505_Data.wingbotDrift.sum_cnt*0.01);     //更新一次偏移量
                   // Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=-Adis16505_Data.wingbotDrift.sum/(Adis16505_Data.wingbotDrift.sum_cnt);     //更新一次偏移量  
                   // Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=Adis16505_Data.Drift_Data.Bit32_Mode.PreviousTrueDrift; 
                    Adis16505_Data.wingbotDrift.max =0;           //清空极大值
                    Adis16505_Data.wingbotDrift.min =0;           //清空极小值  
                    Adis16505_Data.wingbotDrift.sum_cnt=0;        //清空累加次数
                    Adis16505_Data.wingbotDrift.sum=0;            //清空当前一个循环内偏移的累加值
                    DriftTask.task_sum_cnt=0;               
                  }
              }
              DriftTask.time_flag=false;               //ms中断清除
           }
          return Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift;  
      }
      else
      {
       return 0; 
      }
  }
  else
  {
    return 0; 
  }
    
}

u8 Imu_Adis16505 :: getWingbotVel(u8 *hostdata)
{
    if(hostdata[7]==1)          // 整车速度为0
    {
      if(ImuFlag.calibration==true)     //只有当校准完之后才开启定时器中断
      { 
        wingbot.wingbot_vel= 1;   //则该位置1，表示整车静止
        DriftTask.task_flag=true; //开启定时器2的中断  
//        TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断
//        TIM_Cmd(TIM2, ENABLE);  //使能定时器2
      }
    }
    else
    { 
      wingbot.wingbot_vel= 0;   //则该位置0，表示整车运动
      DriftTask.task_flag=false;//关闭定时器2的中断  
//      TIM_ITConfig(TIM2,TIM_IT_Update,DISABLE ); //使能指定的TIM3中断,允许更新中断
//      TIM_Cmd(TIM2, DISABLE);  //使能定时器2
    }
    return 0;
}

float Imu_Adis16505 ::  ImuGetTemperature()               //Imu得到Z轴角速度
{
   int           ret =0;
   float           ret1=0;

   ret = readRegister(ADIS16505_TEMP_OUT) ;               //读取温度
   ret1= ret*0.1;                                         //温度*敏感度
   Adis16505_Data.temperature=ret1;
   return  ret1  ;
}
float Imu_Adis16505 ::  ImuGetTempDrift()                   //Imu得到温度漂移
{
  float drift=0;
  float temp=0;
  temp  = ImuGetTemperature()   ;
  if((temp>=0) &&(temp<=20))
  {
        drift=ImuGetTemperature()*0.001375-0.09;                  //根据温度特性曲线计算出温度漂移值
  }
  else ;
  Adis16505_Data.Drift_Data.tempdrift=drift;
  return drift;
}


float Imu_Adis16505 ::  ImuGetZaxisAnglevel(bool type)               //Imu得到Z轴角速度 
{         
   int           ret =0;
   int           ret4=0;
   unsigned int  ret1=0; 
   unsigned int  ret2=0; 
   unsigned int  ret3=0; 
   float         ret5=0;
   this->BurstReadRegister(Burst32_NumByte);              //发送读数据指令
   ret1 = this->Adis16505_Data.original_.Burst32_Data[6] << 16;
   ret2 = this->Adis16505_Data.original_.Burst32_Data[5];
    
  // ret=  (this->Adis16505_Data.original_.Burst32_Data[6])*pow_int(2,16)+(unsigned int)this->Adis16505_Data.original_.Burst32_Data[5] ;
  // ret= ((this->Adis16505_Data.original_.Burst32_Data[6]) <<16)+(int)this->Adis16505_Data.original_.Burst32_Data[5] ; //原始两个16位数据转换为32位数据 
   ret3 = ret1 + ret2;
   ret  = *(int *)(&ret3);
   ret4 = (int)ret3;
   ret5 =  ret* this->AngleVel_32Bit_Coefficient;
 
//   Movaverage.mv_x.input= ret;             //将原始整数数据导入滤波结构体
//    //传入滤波结构体指针，进行移动平均滤波  原始数据*系数算出原始角速度; 
//    Movaverage.mv_x.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_x)* this->AngleVel_32Bit_Coefficient;  
   if(type==true) 
   {
      return ret5;
   }       
   else
   {
      return ret4 ;
   }
}
/**
 * @brief  Set ADI Accelerometer output data rate when enabled
 * @param  Bit32_Drift: 传入的原始数据
 * @param  times:       要测量的次数,初定为5000个点
 * @retval Imu_Adis16505_STATUS_OK in case of success, an error code otherwise
 */
Imu_Adis16505StatusTypeDef Imu_Adis16505 :: ImuCalibration(int times)    //Imu校准数据，获得静态漂移量
{
    
     float        av=0;
     int          av1=0;
     int          time=100;
     static int drift_cnt=0;              //计数标志，用来计数开机获取数据个数
     float        drift_sum[2]={0u};                  //偏移量的累加和
     float        drift_average[2]={0u};              //偏移量的均值
     int ret[2] ={0u};                     //临时存储数据数组
     int data[3]={0u};                     //临时存储数据数组
     int   ret1;
     float ret2=0;
     this->writeRegister(0x00, ADIS16505_ZG_BIAS_LOW);                  //给初始化偏移寄存器低位写0x00，避免上一次偏移值影响
     this->writeRegister(0x00, ADIS16505_ZG_BIAS_HIGH);                 //给初始化偏移寄存器高位写0x00，避免上一次偏移值影响
 

     for(int i=0;i<times;i++)                                           //循环累加times次，得到累积上电偏移值
     {
         delay_ms(1);
         av=ImuGetZaxisAnglevel(true);
         if(av<=Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0])      //最小值
         {
            Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0]=  av; //  取最小值
         }
         if(av>=Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1])      //最大值
         {
            Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]=  av; //   取最大值
         }
         drift_sum[0]+=av;        

     }
     //去掉两个最大最小值，剩下的点取平均
     drift_sum[0]=drift_sum[0]-Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]-Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0] ;
     drift_average[0]=-(drift_sum[0]/(times-2));       //得到偏移量累加量的均值

     ret[1]= (int(drift_average[0]/AngleVel_32Bit_Coefficient  )) & 0x0000ffff  ;            //得到偏移量的低位
     ret[0]= (int(drift_average[0]/AngleVel_32Bit_Coefficient) >>16)& 0x0000ffff  ;            //得到偏移量的高位

     this->writeRegister(ret[1], ADIS16505_ZG_BIAS_LOW) ;   //写入偏移量低位到寄存器
     data[1]=this->readRegister(ADIS16505_ZG_BIAS_LOW)  ;   //读出寄存器偏移量
     this->writeRegister(ret[0], ADIS16505_ZG_BIAS_HIGH) ;  //写入偏移量高位到寄存器
     data[0]=this->readRegister(ADIS16505_ZG_BIAS_HIGH);    //读出寄存器偏移量     
     delay_ms(10);                                          //写入数据之后等待5ms

     for(int i=0;i<times;i++)                               //循环累加times次，得到累积上电偏移值
     {
         delay_ms(1);       //100
         av=ImuGetZaxisAnglevel(true);
         Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0]=0;
         Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]=0; 
         if(av<=Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0])      //最小值
         {
            Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0]=  av; //  取最小值
         }
         if(av>=Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1])      //最大值
         {
            Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]=  av; //   取最大值
         }
         drift_sum[1]+=av;  
     }

     drift_sum[1]=drift_sum[1]-Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]-Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0] ;
     drift_average[1]=(drift_sum[1]/(times-2));       //得到偏移量累加量的均值 
     
     Adis16505_Data.Drift_Data.Bit32_Mode.Drinft= -drift_average[1];           //更新静态偏移量到结构体

     Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data= -drift_average[1];
     
     /**********清零相关数据，防止校验失败上次结果影响********/
     Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[0]=0;
     Adis16505_Data.Drift_Data.Bit32_Mode.Extreme_Value[1]=0;
     /********************************************************/
     ImuGetTempDrift() ;                               //得到温漂
     if(ImuCheckCalibration())                         //校验静态偏移量是否超过阈值不合理
     {                                                 
       return   Imu_Adis16505_STATUS_ERROR;
     }
     else
     {
        // do{
               Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=0;
               //循环计算10次真实2s钟实际的偏移量
               for(int i=0;i<5000;i++)
               {  
                   delay_ms(1);
                   av1=ImuGetZaxisAnglevel(false);           //得到原始数据
                //   av1=av1+ Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data/ this->AngleVel_32Bit_Coefficient;      //原始数据+静态偏移
                   Movaverage.mv_z.input= av1;             //将原始整数数据导入滤波结构体
           //        Movaverage.mv_z.anglevel=MovingAverage_Filter32(&Movaverage.mv_z)* this->AngleVel_32Bit_Coefficient;        //全值移动平均滤波
                   Movaverage.mv_z.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_z)* this->AngleVel_32Bit_Coefficient; //去掉尖峰移动平均滤波
                   Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift+=(-Movaverage.mv_z.anglevel)*0.001;//每隔10ms积分一次 ，得到1s内静止状态下偏移角度
                 
               }
               Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift/5.0f;    
         //  } while((Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift> DriftDataMax) ||(Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift< DriftDataMin)) ;
       ImuFlag.calibration=true;                         //更新校准标志        
       return   Imu_Adis16505_STATUS_OK;   
     }
          
}   

Imu_Adis16505StatusTypeDef Imu_Adis16505  ::  ImuCheckCalibration()               //Imu获取到静态偏移量后检查偏移量是否正确  
{
    if(Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data> DriftDataMax)
    {
       ImuFlag.calibration=false;                         //更新校准标志
       return   Imu_Adis16505_STATUS_ERROR;
    }
    else if(Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data< DriftDataMin)
    {
       ImuFlag.calibration=false;                         //更新校准标志
       return   Imu_Adis16505_STATUS_ERROR;
    }
    else
    {   
        ImuFlag.calibration=false;  
      //  ImuFlag.calibration=true;                         //更新校准标志
        return   Imu_Adis16505_STATUS_OK; 
    }
}

Imu_Adis16505StatusTypeDef Imu_Adis16505 :: ImuAcc_32Bit_DataHandle(u16 *OriginalData)   //原始32位数据的值进行转化
{
    float BufferData[10] = {0};
    int   Data_32Bit[10] = {0};
    for (int i = 0; i < 3; i++)
    {
        Data_32Bit[1 + i]= (int )((u32(OriginalData[2*i+2]) <<16)+OriginalData[2*i+1] );           //原始两个16位数据转换为32位数据

        if ((Data_32Bit[1 + i] <= 1310720000 ) && (Data_32Bit[1 + i] >= (-1310720000)))           //判断是否超过传感器测量范围
        {              
            //对原始角速度做一阶低通滤波
            const float factor = 0.01f;  //滤波因素 
            static float tBuff[3];    
            tBuff[i] = tBuff[i] * (1 - factor) + ((float)Data_32Bit[1 + i] )* factor;       // 一阶低通滤波

            if(i==2)        //对Z轴做滤波
            {
               Movaverage.mv_z.input= Data_32Bit[1 + i];             //将原始整数数据导入滤波结构体
           //    Movaverage.mv_z.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_z)* this->AngleVel_32Bit_Coefficient;  
               Movaverage.mv_z.anglevel=MovingAverage_Filter32(&Movaverage.mv_z)* this->AngleVel_32Bit_Coefficient;
               BufferData[1 + i]=Movaverage.mv_z.anglevel ;
               //对原始数据做一阶低通滤波，保存到低通滤波结构体中  
               LowpassFilter.Z_Axis.AngelVel =   tBuff[i] * this->AngleVel_32Bit_Coefficient;  //  原始数据*系数算出原始角速度
            }
        }
        else        
        {
            if(Data_32Bit[1 + i]>0 )
            {
                BufferData[1 + i] = 125 ;       //超出范围若为正则判断为达到最大速度125°/s;
                
            }
            else
            {
                BufferData[1 + i] = -125 ;      //超出范围若为负则判断为达到最大速度-125°/s
            }

        //    printf("陀螺仪数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }

    Adis16505_Data.original_.anglevel=Data_32Bit[3]* this->AngleVel_32Bit_Coefficient;  //  原始数据*系数算出原始角速度;
    
    if((LowpassFilter.Z_Axis.AngelVel<0.02) && (LowpassFilter.Z_Axis.AngelVel>-0.02))
    {  
       LowpassFilter.Z_Axis.AngelVel=0;         //如果是静态偏移则置为0
    }
    else
        LowpassFilter.Z_Axis.AngelVel=LowpassFilter.Z_Axis.AngelVel;

    //IMU  Z轴角速度进行积分
    if(Integr_Flag==1)    // Integr_Flag=1表示定时器15ms中断已到进行积分
    {    
        if(ImuFlag.calibration==true)   //Imu获取数据静态漂移量
        {    
            //一重积分算出线速度和角度
            this->Adis16505_Data.Z_Axis.AngelVel = Adis16505_Data.original_.anglevel+Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift;
//             Movaverage.mv_z.angl+=(Movaverage.mv_z.anglevel+Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift)*Calculus_Time;      //每隔10ms积分一次
            this->Adis16505_Data.Z_Axis.Angle+=this->Adis16505_Data.Z_Axis.AngelVel*Calculus_Time;  
//           Movaverage.mv_z.angl+=(Movaverage.mv_z.anglevel+Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data)*Calculus_Time;  //每隔10ms积分一次
             //一重低通滤波角度
             Adis16505_Data.Z_Axis.DeltaAngle+=(LowpassFilter.Z_Axis.AngelVel+Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift)*Calculus_Time;//每隔10ms积分一次
             LowpassFilter.Z_Axis.Angle+=LowpassFilter.Z_Axis.AngelVel*Calculus_Time;//每隔15ms积分一次
            
////             if(DriftTask.task_flag== true)                     //判断wingbot是否静止 ，若静止，低通滤波权重更大
////             {
////               this->Adis16505_Data.Z_Axis.Angle= Movaverage.mv_z.angl*0.1+LowpassFilter.Z_Axis.Angle*0.9- this->Adis16505_Data.F4_data.Offest_Data ;
////             }
////             else                                               //若不静止，移动平均滤波权重更大
////             {
////               this->Adis16505_Data.Z_Axis.Angle= Movaverage.mv_z.angl*0.9+LowpassFilter.Z_Axis.Angle*0.1- this->Adis16505_Data.F4_data.Offest_Data ;
////             }
             ImuUpdateDrift();
        }
        else
             this->Adis16505_Data.Z_Axis.Angle=0;    
        
       Integr_Flag=0;  //清零定时器标志位标志位
    }
     return Imu_Adis16505_STATUS_OK;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505::Adis16505SetReadMode(u16 mode)
{
    u16 ret = 0;
    while (ret != mode)
    {
        this->writeRegister(mode, ADIS16505_MSC_CTRL); //配置为32位读取加速度模式
        delay_us(100);
        ret = readRegister(ADIS16505_MSC_CTRL);
    }
    return Imu_Adis16505_STATUS_OK;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505::BurstReadRegister(u16 BYTE_NUM)
{
    if (BYTE_NUM == Burst32_NumByte)
    {
        s16 AG[Burst32_NumByte] = {0};
        u16 CHECK_SUM = 0x0000;
        _ptrans->chipSelect();
        _ptrans->readAfterWrite(ADIS16505_BURST_FLAG & 0x7F00);
        for (u8 i = 0; i < Burst32_NumByte; i++)
        {
            AG[i] = _ptrans->readAfterWrite(0x0000);
        }
        _ptrans->chipRelease();
        /**************计算校验和*************/
        for (u8 i = 0; i < Burst32_NumByte - 1; i++)
        {
            //CHECK_SUM+=((AG[i]>>8)+(AG[i]&0x00ff));
            CHECK_SUM += ((((u16)AG[i]) >> 8) + (((u16)AG[i]) & 0x00ff));
        }
        /**************************************/
        /**********************判断校验和******************/
        if (AG[Burst32_NumByte - 1] == CHECK_SUM)
        {
            for (u8 i = 0; i < Burst32_NumByte; i++)
            {
                this->Adis16505_Data.original_.Burst32_Data[i] = AG[i];
            }
            return Imu_Adis16505_STATUS_OK;
        }
        else
        {
            return Imu_Adis16505_STATUS_ERROR;
        }
    }
    else
    {
        s16 AG[10] = {0};
        u16 CHECK_SUM = 0x0000;
        _ptrans->chipSelect();
        _ptrans->readAfterWrite(ADIS16505_BURST_FLAG & 0x7F00);
        for (u8 i = 0; i < Burst16_NumByte; i++)
        {
            AG[i] = _ptrans->readAfterWrite(0x0000);
        }
        _ptrans->chipRelease();

        /**************计算校验和*************/
        for (u8 i = 0; i < Burst16_NumByte - 1; i++)
        {
            CHECK_SUM += ((((u16)AG[i]) >> 8) + (((u16)AG[i]) & 0x00ff));
        }
        /**************************************/
        /**********************判断校验和******************/
        if (AG[Burst16_NumByte - 1] == CHECK_SUM)
        {
            for (u8 i = 0; i < Burst16_NumByte; i++)
            {
                Adis16505_Data.original_.Burst16_Data[i] = AG[i];
            }
            //            this->ImuDataConversion(AG);
            return Imu_Adis16505_STATUS_OK;
        }
        else
        {
            return Imu_Adis16505_STATUS_ERROR;
        }
    }
}

Imu_Adis16505StatusTypeDef Imu_Adis16505::ImuAccDataConversion(u16 *OriginalData) //IMU
{

    float BufferData[10] = {0};
    for (int i = 0; i < 3; i++)
    {
        if ((((s16)OriginalData[1 + i]) >= -20000) && (((s16)OriginalData[1 + i]) <= 20000))
        {

            BufferData[1 + i] = ((float)((s16)OriginalData[1 + i])) * this->AngleVelCoefficient; //单位为°/s
        }
        else
        {
            printf("陀螺仪数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }

    for (int i = 0; i < 3; i++)
    {
        if ((((s16)OriginalData[4 + i]) <= 32000) && (((s16)OriginalData[4 + i]) >= -32000))
        {
            BufferData[4 + i] = ((float)((s16)OriginalData[4 + i])) * this->AccCoefficient / 1000.0; //单位位m/s
        }
        else
        {
            printf("加速度数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }
    this->Adis16505_Data.X_Axis.AngelVel = BufferData[1];
    this->Adis16505_Data.Y_Axis.AngelVel = BufferData[2];
    this->Adis16505_Data.Z_Axis.AngelVel = BufferData[3];
    this->Adis16505_Data.X_Axis.LineAcc = BufferData[4];
    this->Adis16505_Data.Y_Axis.LineAcc = BufferData[5];
    this->Adis16505_Data.Z_Axis.LineAcc = BufferData[6];
    
    //IMU  Z轴角速度进行积分
    if(Integr_Flag==1)    // Integr_Flag=1表示定时器10ms中断已到进行积分
    {    
        
         this->Adis16505_Data.Z_Axis.Angle+=this->Adis16505_Data.Z_Axis.AngelVel*Calculus_Time;//每隔10ms积分一次
         Integr_Flag=0;  //清零定时器标志位标志位
         OffestSelf(0x019,0x0fff,OriginalData[3]); //Imu自身校准数据
    }
    
    return Imu_Adis16505_STATUS_OK;

}

Imu_Adis16505StatusTypeDef Imu_Adis16505::ImuAcc_16Bit_DataConversion(u16 *OriginalData)   //原始16位数据的值进行转化
{
    float BufferData[10] = {0};
    for (int i = 0; i < 3; i++)
    {
        if ((((s16)OriginalData[1 + i]) >= -20000) && (((s16)OriginalData[1 + i]) <= 20000))
        {

     //       BufferData[1 + i] = ((float)((s16)OriginalData[1 + i])) * this->AngleVelCoefficient; //单位为°/s
            
             BufferData[1 + i] =((float)OriginalData[1 + i] );
            //对原始角速度做一阶低通滤波
            uint8_t k=i-3;
            const float factor = 0.15f;  //滤波因素      
            static float tBuff[3];    
             tBuff[i] = tBuff[i] * (1 - factor) + BufferData[1 + i]* factor;   
             BufferData[1 + i] =   tBuff[i] * this->AngleVelCoefficient;  //  原始数据*系数算出原始角速度
        }
        else
        {
            printf("陀螺仪数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }

    for (int i = 0; i < 3; i++)
    {
        if ((((s16)OriginalData[4 + i]) <= 32000) && (((s16)OriginalData[4 + i]) >= -32000))
        {
         //   BufferData[4 + i] = ((float)((s16)OriginalData[4 + i])) * this->AccCoefficient / 1000.0; //单位位m/s
            
            BufferData[4 + i] = ((float) OriginalData[4 + i] )  ;
            //对加速度做一维卡尔曼滤波
            static struct _1_ekf_filter ekf[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};  
            kalman_1(&ekf[i],(float)BufferData[4 + i] );  //一维卡尔曼
            BufferData[4 + i]  = (int16_t)ekf[i].out ;
            BufferData[4 + i]  = BufferData[4 + i]  * this->AccCoefficient / 1000.0; //单位位m/s;  
            
        }
        else
        {
            printf("加速度数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }
    this->Adis16505_Data.X_Axis.AngelVel = BufferData[1];
    this->Adis16505_Data.Y_Axis.AngelVel = BufferData[2];
    this->Adis16505_Data.Z_Axis.AngelVel = BufferData[3];//-Adis16505_Data.Drift_Data.Original__Data;  //  减去静态偏移得到最终角速度
    this->Adis16505_Data.X_Axis.LineAcc = BufferData[4];
    this->Adis16505_Data.Y_Axis.LineAcc = BufferData[5];
    this->Adis16505_Data.Z_Axis.LineAcc = BufferData[6];
    
    //IMU  Z轴角速度进行积分
    if(Integr_Flag==1)    // Integr_Flag=1表示定时器10ms中断已到进行积分
    {    
        
         this->Adis16505_Data.Z_Axis.Angle+=this->Adis16505_Data.Z_Axis.AngelVel*Calculus_Time;//每隔10ms积分一次
         Integr_Flag=0;  //清零定时器标志位标志位
        // OffestSelf(0x1b,0xfff,OriginalData[3]); //Imu自身校准数据
         OffestSelf(0x0000,0x0000,OriginalData[3],0); //Imu自身校准数据
    }
    return Imu_Adis16505_STATUS_OK;
}


Imu_Adis16505StatusTypeDef Imu_Adis16505::ImuAcc_32Bit_DataConversion(u16 *OriginalData)  //原始32位数据的值进行转化
{
    float ZZ=0;   //临时角速度
    float CC=0;   //临时角速度
    float BufferData[10] = {0};
    
    int   Data_32Bit[10] ={0};
    for (int i = 0; i < 3; i++)
    {
        Data_32Bit[1 + i]= (((int)OriginalData[2*i+2]) <<16)+(int)OriginalData[2*i+1] ;           //原始两个16位数据转换为32位数据
        if ((Data_32Bit[1 + i] <= 1310720000 ) && (Data_32Bit[1 + i] >= (-1310720000)))           //判断是否超过传感器测量范围
        {
            //对原始角速度做一阶低通滤波
            const float factor = 0.01f;  //滤波因素 
            static float tBuff[3];    
            tBuff[i] = tBuff[i] * (1 - factor) + ((float)Data_32Bit[1 + i] )* factor;       // 一阶低通滤波
//          BufferData[1 + i] =   tBuff[i] * this->AngleVel_32Bit_Coefficient;  //  原始数据*系数算出原始角速度
            if(i==0)         //对X轴做滤波
            {
               Movaverage.mv_x.input= Data_32Bit[1 + i];             //将原始整数数据导入滤波结构体
               //传入滤波结构体指针，进行移动平均滤波  原始数据*系数算出原始角速度; 
               Movaverage.mv_x.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_x)* this->AngleVel_32Bit_Coefficient;  
               BufferData[1 + i]=Movaverage.mv_x.anglevel ;
               //对原始数据做一阶低通滤波，保存到低通滤波结构体中 
               LowpassFilter.X_Axis.AngelVel= tBuff[0] * this->AngleVel_32Bit_Coefficient; 
            }
            if(i==1)        //对Y轴做滤波
            {
               Movaverage.mv_y.input= Data_32Bit[1 + i];             //将原始整数数据导入滤波结构体
               //传入滤波结构体指针，进行移动平均滤波  原始数据*系数算出原始角速度; 
               Movaverage.mv_y.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_y)* this->AngleVel_32Bit_Coefficient;   
               BufferData[1 + i]=Movaverage.mv_y.anglevel ;
               //对原始数据做一阶低通滤波，保存到低通滤波结构体中 
               LowpassFilter.Y_Axis.AngelVel= tBuff[1] * this->AngleVel_32Bit_Coefficient; 
            }
            if(i==2)        //对Z轴做滤波
            {
               Movaverage.mv_z.input= Data_32Bit[1 + i];             //将原始整数数据导入滤波结构体
               //传入滤波结构体指针，进行移动平均滤波  原始数据*系数算出原始角速度; 
               Movaverage.mv_z.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_z)* this->AngleVel_32Bit_Coefficient;  
               BufferData[1 + i]=Movaverage.mv_z.anglevel ;
               //对原始数据做一阶低通滤波，保存到低通滤波结构体中  
               LowpassFilter.Z_Axis.AngelVel =   tBuff[i] * this->AngleVel_32Bit_Coefficient;  //  原始数据*系数算出原始角速度
            }
//            BufferData[1 + i] =((float)Data_32Bit[1 + i] );
//            //对原始角速度做一阶低通滤波
//            uint8_t k=i-3;
//            const float factor = 0.01f;  //滤波因素 
//            static float tBuff[3];    
//            tBuff[i] = tBuff[i] * (1 - factor) + BufferData[1 + i]* factor;   
//            BufferData[1 + i] =   tBuff[i] * this->AngleVel_32Bit_Coefficient;  //  原始数据*系数算出原始角速度
        }
        else        
        {
            if(Data_32Bit[1 + i]>0 )
            {
                BufferData[1 + i] = 125 ;       //超出范围若为正则判断为达到最大速度125°/s;
                
            }
            else
            {
                BufferData[1 + i] = -125 ;      //超出范围若为负则判断为达到最大速度-125°/s
            }

            printf("陀螺仪数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }

    for (int i = 0; i < 3; i++)
    {
        Data_32Bit[4 + i]= (((int)OriginalData[2*i+8] )<<16)+(int)OriginalData[2*i+7] ;
        if ((Data_32Bit[4 + i] <= 2097152000) && (Data_32Bit[4 + i] >= -2097152000))
        {
        //  BufferData[4 + i] = ((float) Data_32Bit[4 + i] ) * this->Acc_32Bit_Coefficient / 1000.0; //单位位m/s
            BufferData[4 + i] = ((float) Data_32Bit[4 + i] )  ;
            //对加速度做一维卡尔曼滤波
            static struct _1_ekf_filter ekf[3] = {{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543},{0.02,0,0,0,0.001,0.543}};  
            kalman_1(&ekf[i],(float)BufferData[4 + i] );  //一维卡尔曼
            BufferData[4 + i]  = (int32_t)ekf[i].out ;
            BufferData[4 + i]  = BufferData[4 + i]  * this->Acc_32Bit_Coefficient / 1000.0; //单位位m/s; 
        }
        else
        {
            printf("加速度数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }
    
    ZZ=((float)Data_32Bit[3])* this->AngleVel_32Bit_Coefficient;
    this->Adis16505_Data.X_Axis.AngelVel = BufferData[1];
    this->Adis16505_Data.Y_Axis.AngelVel = BufferData[2];
    Adis16505_Data.original_.anglevel=BufferData[3];
    
    if((LowpassFilter.Z_Axis.AngelVel<0.08) && (LowpassFilter.Z_Axis.AngelVel>-0.08))
    {  
       CC =0;                   //如果是静态偏移则置为0
    }
    else
        CC=LowpassFilter.Z_Axis.AngelVel;
    if((BufferData[3]<0.08) && (BufferData[3]>-0.08))
    {  
       BufferData[3] =0;                   //如果是静态偏移则置为0
    }
    else  ;
   //   BufferData[3] = abs(Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data ) +   BufferData[3];      //若>0.05°/s  ，则视为运动，需加上静态漂移
   
    if((BufferData[5]<0.07) && (BufferData[5]>-0.07))
    {  
       BufferData[5] =0;
    }

    this->Adis16505_Data.Z_Axis.AngelVel = BufferData[3] ;//- this->Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data;
    this->Adis16505_Data.X_Axis.LineAcc = BufferData[4];
    this->Adis16505_Data.Y_Axis.LineAcc = BufferData[5];
    this->Adis16505_Data.Z_Axis.LineAcc = BufferData[6];
    

    //IMU  Z轴角速度进行积分
    if(Integr_Flag==1)    // Integr_Flag=1表示定时器10ms中断已到进行积分
    {    
        OffestSelf_32Bit(0x0000,0x0000 ,Data_32Bit); //Imu 32Bit 自身校准数据
     // if(Drinft_1S_flag==1)   //1s校准标志位结束，表示校准OK
        if(ImuGet_Drift(Data_32Bit)==Imu_Adis16505_STATUS_OK)   //Imu获取数据静态漂移量
        {    
             float a=0;
             this->Adis16505_Data.Z_Axis.Distance+=this->Adis16505_Data.Z_Axis.AngelVel*Calculus_Time;//每隔10ms积分一次
             if(this->Adis16505_Data.Z_Axis.AngelVel>0.08)    //若角速度在正负0.05之外则视为运动，把静态补偿加入
             {   
                 this->Adis16505_Data.Z_Axis.AngelVel =   this->Adis16505_Data.Z_Axis.AngelVel+Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data*0.8 + 0.96*0.01  ;       //加入静态补偿
                 a =   this->Adis16505_Data.Z_Axis.AngelVel-0.01*0.95   ;       //加入静态补偿
             }
             if(this->Adis16505_Data.Z_Axis.AngelVel<-0.08)
             {
                 this->Adis16505_Data.Z_Axis.AngelVel =   this->Adis16505_Data.Z_Axis.AngelVel-Adis16505_Data.Drift_Data.Bit32_Mode.Original__Data*0.8 -0.96*0.01  ;       //加入静态补偿
                 a =   this->Adis16505_Data.Z_Axis.AngelVel+0.01*0.95   ;       //加入静态补偿
             }
            //一重积分算出线速度和角度
             this->Adis16505_Data.Z_Axis.DeltaAngle+=a*Calculus_Time;//每隔10ms积分一次             
             this->Adis16505_Data.Z_Axis.Angle+=this->Adis16505_Data.Z_Axis.AngelVel*Calculus_Time;//每隔10ms积分一次
             this->Adis16505_Data.X_Axis.LineVel+=this->Adis16505_Data.X_Axis.LineAcc*Calculus_Time;//每隔10ms积分一次
             this->Adis16505_Data.Y_Axis.LineVel+=this->Adis16505_Data.Y_Axis.LineAcc*Calculus_Time;//每隔10ms积分一次
             this->Adis16505_Data.Z_Axis.LineVel+=this->Adis16505_Data.Z_Axis.LineAcc*Calculus_Time;//每隔10ms积分一次
             //一重移动平均滤波角度
             Movaverage.mv_z.angl+=Movaverage.mv_z.anglevel*Calculus_Time;//每隔10ms积分一次
             //一重低通滤波角度
             LowpassFilter.Z_Axis.Angle+=CC*Calculus_Time;//每隔10ms积分一次
             
            //二重积分算出；路程
    //         this->Adis16505_Data.X_Axis.Distance+=this->Adis16505_Data.X_Axis.LineVel*Calculus_Time;//每隔10ms积分一次
    //         this->Adis16505_Data.Y_Axis.Distance+=this->Adis16505_Data.Y_Axis.LineVel*Calculus_Time;//每隔10ms积分一次
//             printf("Orginal AngelVel: %f    Filter Angelvel: %f   Threshold Angelvel: %f   Real Angle: %f   hour%d min%d sec%d msec%d \r\n",ZZ,Adis16505_Data.original_.anglevel,\
//                     this->Adis16505_Data.Z_Axis.AngelVel,this->Adis16505_Data.Z_Axis.Angle,ImuTime.hour, \
//                     ImuTime.min, ImuTime.sec,ImuTime.msec);
//             printf(" ms%d \r\n",ImuTime.msec); 
//            printf("Orginal data: %f  Filter Angelvel: %f\r\n"  ,  ((float)Data_32Bit[3]), Adis16505_Data.original_.anglevel);
              ReportData(0x52,short(Movaverage.mv_z.anglevel*1000/0.061f),short(LowpassFilter.Z_Axis.AngelVel*1000/0.061f),short(ZZ*1000/0.061f),0) ;
//            ReportData(0x53,short(Movaverage.mv_z.angl*1000/0.55f),short(LowpassFilter.Z_Axis.Angle*1000/0.55f),short(this->Adis16505_Data.Z_Axis.Angle*1000/0.55f),0) ; 
              printf("\r\n ms%d \r\n",ImuTime.msec);    
          //  ReportData(0x52,short(0x0001/0.061f),0x0002,0x0003,0) ;   
          //  ReportData(0x53,(0x0001)*100/0.55f,0x0003,0x0003,0) ;                       
        }
        else
             this->Adis16505_Data.Z_Axis.Angle=0;    
        
       Integr_Flag=0;  //清零定时器标志位标志位
    }
     return Imu_Adis16505_STATUS_OK;
 
}





Imu_Adis16505StatusTypeDef Imu_Adis16505::ImuDeltaDataConversion(u16 *OriginalData)
{
    float BufferData[10] = {0};
    for (int i = 0; i < 3; i++)
    {
        if ((((s16)OriginalData[1 + i]) >= -32768) && (((s16)OriginalData[1 + i]) <= 32767))
        {
            BufferData[1 + i] = ((float)((s16)OriginalData[1 + i])) * this->DeltaAngleCoefficient; //单位为°/s
        }
        else
        {
            printf("Delta角度数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }

    for (int i = 0; i < 3; i++)
    {
        if ((((s16)OriginalData[4 + i]) <= 32767) && (((s16)OriginalData[4 + i]) >= -32768))
        {
            BufferData[4 + i] = ((float)((s16)OriginalData[4 + i])) * this->DeltaVelCoefficient; //单位位m/s
        }
        else
        {
            printf("加速度数据错误，不在变化范围内");
            return Imu_Adis16505_STATUS_ERROR;
        }
    }
    this->Adis16505_Data.X_Axis.DeltaAngle = BufferData[1];
    this->Adis16505_Data.Y_Axis.DeltaAngle = BufferData[2];
    this->Adis16505_Data.Z_Axis.DeltaAngle = BufferData[3];
    this->Adis16505_Data.X_Axis.DeltaVel = BufferData[4];
    this->Adis16505_Data.Y_Axis.DeltaVel = BufferData[5];
    this->Adis16505_Data.Z_Axis.DeltaVel = BufferData[6];
    return Imu_Adis16505_STATUS_OK;
}



Imu_Adis16505StatusTypeDef Imu_Adis16505 ::ImuData_Clear()   //Imu数据清零
{
    this->Adis16505_Data.Z_Axis.AngelVel =0;
    this->Adis16505_Data.Z_Axis.Angle=0;
    Movaverage.mv_z.angl=0;
    LowpassFilter.Z_Axis.Angle=0; 
    this->Adis16505_Data.F4_data.Offest_Data =0;
    Zero_Status[4] = 0x01;     //更新IMU清零的状态，1表示清零成功
 //   memset(this->Adis16505_Data.Z_Axis, 0, sizeof( Adis16505_Data.Z_Axis));
    return Imu_Adis16505_STATUS_OK;
}                              

//IMU通过串口返回F4实时位置和角度数据的函数  24个字节
Imu_Adis16505StatusTypeDef Imu_Adis16505 ::Adis16505_Return_F4_PositionData(u8 *order)
{

    u8 sum = 0;
    u8 i = 0;
    int AngleData = 0;
    float* fAngle;
    Adis16505_Data.original_.ImuSendReturnData[0] = 0x0d;
    Adis16505_Data.original_.ImuSendReturnData[1] = 0x0a;
    //判断是否为修正角度命令(0x06)或者返回实时位置命令（0x04）
    if ((order[0] == 0x0d) && (order[1] == 0x0a) && (order[2] == 0x06) && (order[adis16505_RX_data_len - 2] == 0x5a) && (order[adis16505_RX_data_len - 1] == 0xa5))
    {
        Adis16505_Data.original_.ImuSendReturnData[2] = 0x06;
    }
    else
    {
        Adis16505_Data.original_.ImuSendReturnData[2] = 0x04;
    }
    for (i = 0; i < 4; i++)
    {
        Adis16505_Data.original_.ImuSendReturnData[3+i] = 0x00; //1号IMU数据为0（LSM6DSLTR）
    }

    /***************填充2号IMU数据将float型原始数据*1000转换为整型，拆分为4个字节放入返回数组*********/

    fAngle = (float*)&Adis16505_Data.original_.ImuSendReturnData[7];
    *fAngle = Adis16505_Data.Z_Axis.Angle;

    /***************************************************************************************************/
    for (i = 0; i < 2; i++)
    {
        for (u8 j = 0; j < 4; j++)
        {
            Adis16505_Data.original_.ImuSendReturnData[11 + i * 4 + j] = 0x00; //其余预留位填充为0
        }
    }
    Adis16505_Data.original_.ImuSendReturnData[adis16505_Returndata_len - 4] = 0x00;
    Adis16505_Data.original_.ImuSendReturnData[adis16505_Returndata_len - 5] = 0x00;

    for (i = 0; i < (adis16505_Returndata_len - 5); i++)
        sum += Adis16505_Data.original_.ImuSendReturnData[2 + i]; //CRC校验，把除（包头包尾和校验位）的所有数据累加
    Adis16505_Data.original_.ImuSendReturnData[adis16505_Returndata_len - 3] = sum;
    Adis16505_Data.original_.ImuSendReturnData[adis16505_Returndata_len - 2] = 0x5a;
    Adis16505_Data.original_.ImuSendReturnData[adis16505_Returndata_len - 1] = 0xa5;

    UART3_Send_Array(Adis16505_Data.original_.ImuSendReturnData, adis16505_Returndata_len);

    Frame_Sign_usart3.now_status = Usart_Initial_number;
    //  printf("发送成功\r\n");
    // for(i=0;i<adis16505_Returndata_len;i++)
    // printf("Adis16505_Send_Return_data[%d]:%d\r\n",i,Adis16505_Send_Return_data[i]);

    // printf("%x",Adis16505_Send_Return_data[i]);
    // printf("Adis16505_Send_Return_data[%d]:%d\r\n",i,Adis16505_Send_Return_data[i]);
    //  printf("发送成功\r\n");

    return Imu_Adis16505_STATUS_OK;
}

//IMU通过串口返回F4实时位置角度清零是否成功的函数    12个字节
Imu_Adis16505StatusTypeDef Imu_Adis16505 ::Adis16505_Return_F4_ZeroData()
{                                               
    u8 sum = 0;
    u8 i = 0;

    Adis16505_Data.original_.ImuSendReturnData[0] = 0x0d;
    Adis16505_Data.original_.ImuSendReturnData[1] = 0x0a;
    Adis16505_Data.original_.ImuSendReturnData[2] = 0x02;
    for (i = 0; i < 2; i++)
    {
        Adis16505_Data.original_.ImuSendReturnData[3 + i] = this->Adis16505_Data.ZeroState.Zero_Flag[i];
    }
    for (i = 0; i < 4; i++)
    {
        Adis16505_Data.original_.ImuSendReturnData[5 + i] = 0x00;
    }

    for (i = 0; i < 7; i++)
        sum += Send_Retutn_data[2 + i];
    Adis16505_Data.original_.ImuSendReturnData[9] = sum;
    Adis16505_Data.original_.ImuSendReturnData[10] = 0x5a;
    Adis16505_Data.original_.ImuSendReturnData[11] = 0xa5;

    UART3_Send_Array(Adis16505_Data.original_.ImuSendReturnData, 12);
    for (i = 0; i < 2; i++)
    {
        this->Adis16505_Data.ZeroState.Zero_Flag[i] = 0x00;
    }
    Frame_Sign_usart3.now_status = Usart_Initial_number; //代表一帧发送结束

    return Imu_Adis16505_STATUS_OK;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505 ::Adis16505_Analysis_F4_Order(u8 *order) //在解析到F4下发的指令后解析指令的具体内容,参数为F4下发的串口数据
{

    if (order[2] == F4_Angle_Imu_Order)
    {
        if (order[4] == 1)
            Adis16505_Return_F4_PositionData(usart3_Receive_Data);
        else
            ;
    }
    else if (order[2] == F4_Zero_Imu_Order)
    {
        if (order[4] == 1)
            Adis16505_Return_F4_ZeroData();
        else
            ;
    }
    else if (order[2] == F4_OffestAngle_Imu_Order)
    {
        if (order[4] == 1)
        {
            Adis16505_F4_Offest_Position(usart3_Receive_Data);
            Adis16505_Return_F4_PositionData(usart3_Receive_Data);
        }
        else
         ;    
    }
    else
    {
         ;
    }
    Frame_Sign_usart3.now_status = Usart_Initial_number;  //串口指令处理完的清零标志
    return Imu_Adis16505_STATUS_OK;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505 ::Adis16505_F4_Offest_Position(u8 *order) //在解析到F4下发的指令后,执行F4下发的校准指令，内容位修正的角度位置
{
    int F4_offest = 0;
    for (u8 i = 0; i < 4; i++)
    {
        F4_offest += (u32)order[5 + i] << (i * 8);
    }
//    this->Adis16505_Data.F4_data.Offest_Data =this->Adis16505_Data.F4_data.Offest_Data+ F4_offest / 1000.0f;
//    this->Adis16505_Data.F4_data.Original__Data = F4_offest;
//   
//    this->Adis16505_Data.Z_Axis.Angle=this->Adis16505_Data.Z_Axis.Angle- this->Adis16505_Data.F4_data.Offest_Data   ;
    F4_offest=F4_offest / 1000.0f;
    this->Adis16505_Data.Z_Axis.Angle=this->Adis16505_Data.Z_Axis.Angle + F4_offest;
    Frame_Sign_usart3.now_status = Usart_Initial_number;
    return Imu_Adis16505_STATUS_OK;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505::BurstModeAcc_16Bit() //16位爆裂读模式 默认16位读模式，burst_sel=0, disable
{
    Imu_Adis16505StatusTypeDef ret;
    if (this->Adis16505SetReadMode(ADIS16505_BIT16_ACC) == Imu_Adis16505_STATUS_OK)
    {
        this->BurstReadRegister(Burst16_NumByte);
        ret = this->ImuAccDataConversion(Adis16505_Data.original_.Burst16_Data);
        ret = this->ImuAcc_16Bit_DataConversion(Adis16505_Data.original_.Burst16_Data) ; //原始32位数据的值进行转化
    }
    else
        return Imu_Adis16505_STATUS_ERROR;
    return ret;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505::BurstModeAcc_32Bit() //32位爆裂读模式
{
    Imu_Adis16505StatusTypeDef ret;
    this->BurstReadRegister(Burst32_NumByte);                                           //获取原始数据

    if(ImuFlag.calibration==true)                                                       //判断IMU是否完成开机校准
    {
      ret = this->ImuAcc_32Bit_DataHandle(Adis16505_Data.original_.Burst32_Data) ;       //原始32位数据的值进行转化
    }
    return  ret;    
}

Imu_Adis16505StatusTypeDef Imu_Adis16505::BurstModeDelta_16Bit() //16位爆裂读模式
{
    Imu_Adis16505StatusTypeDef ret;
    if (this->Adis16505SetReadMode(ADIS16505_BIT16_ANGLE) == Imu_Adis16505_STATUS_OK)
    {

        this->BurstReadRegister(Burst16_NumByte);
        
        ret = this->ImuDeltaDataConversion(Adis16505_Data.original_.Burst16_Data);
    }
    else
        return Imu_Adis16505_STATUS_ERROR;
    return ret;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505::BurstModeDelta_32Bit() //32位爆裂读模式
{
    if (this->Adis16505SetReadMode(ADIS16505_BIT32_ANGLE) == Imu_Adis16505_STATUS_OK)
    {
        return this->BurstReadRegister(Burst32_NumByte);
    }
    else
        return Imu_Adis16505_STATUS_ERROR;
}



Imu_Adis16505StatusTypeDef Imu_Adis16505::OffestSelf_ACC_32Bit(u16 OffestHigh,u16 OffestLow ,int XACC_Bit32_Drift,int YACC_Bit32_Drift) //Imu 32Bit 自身校准数据
{
     return  Imu_Adis16505_STATUS_ERROR;
}

Imu_Adis16505StatusTypeDef Imu_Adis16505:: OffestSelf(u16 OffestHigh,u16 OffestLow ,s16 Drift,int Bit32_Drift)
{
     return  Imu_Adis16505_STATUS_ERROR;
}
Imu_Adis16505StatusTypeDef Imu_Adis16505:: OffestSelf_32Bit(u16 OffestHigh,u16 OffestLow ,int* Bit32_Drift) //Imu 32Bit 自身校准数据
{
     return  Imu_Adis16505_STATUS_ERROR;
}

//float Imu_Adis16505 :: ImuUpdateDrift()
//{                                     
//  if(ImuFlag.calibration==true)  
//  { int av1=0;
//  
//   if(DriftTask.time_flag==true)
//   {
//      av1=ImuGetZaxisAnglevel(false);           //得到原始数据
//      Adis16505_Data.wingbotDrift.anglevel=av1 * this->AngleVel_32Bit_Coefficient;             //得到原始角速度
//      Movaverage.mv_z.input= av1;             //将原始整数数据导入滤波结构体
//      //Movaverage.mv_z.anglevel=MovingAverage_Filter32(&Movaverage.mv_z)* this->AngleVel_32Bit_Coefficient;         //全值移动平均滤波
//      Movaverage.mv_z.anglevel=AntiPulse_MovingAverage_Filter32(&Movaverage.mv_z)* this->AngleVel_32Bit_Coefficient; //抗干扰型移动平均滤波
//      Adis16505_Data.wingbotDrift.filteranglevel= Movaverage.mv_z.anglevel;
//      Adis16505_Data.wingbotDrift.sum+=Movaverage.mv_z.anglevel*DriftTask.time;     //10ms中断;
//      Adis16505_Data.wingbotDrift.sum_cnt++;                 //偏移累加量+1
//      DriftTask.task_sum_cnt++;                              //进入中断次数+1
//      if(Adis16505_Data.wingbotDrift.sum_cnt==100)           //如果1s钟时间到
//      {
//        Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=-Adis16505_Data.wingbotDrift.sum;     //更新一次偏移量
//        Adis16505_Data.wingbotDrift.sum_cnt=0;
//        Adis16505_Data.wingbotDrift.sum=0;  
//        DriftTask.task_sum_cnt=0;                          
//          
//      }
//      else                                                    //如果1s钟时间未到
//      {
//          if(DriftTask.task_flag== false)                     //判断wingbot是否静止
//          { 
//            //如果开始运动，则结算当前时间偏移量
//            Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift=-Adis16505_Data.wingbotDrift.sum/(Adis16505_Data.wingbotDrift.sum_cnt*DriftTask.time);     //更新一次偏移量
//            Adis16505_Data.wingbotDrift.sum_cnt=0;
//            Adis16505_Data.wingbotDrift.sum=0;  
//            DriftTask.task_sum_cnt=0;               
//          }
//      }
//      DriftTask.time_flag=false;               //ms中断清除
//   }
////   if( DriftTask.task_flag== true)
////   {
////     TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断
////     TIM_Cmd(TIM2, ENABLE);  //使能定时器2
////   }
////   else
////   {
////     TIM_ITConfig(TIM2,TIM_IT_Update,DISABLE ); //使能指定的TIM3中断,允许更新中断
////     TIM_Cmd(TIM2, DISABLE);  //使能定时器2
////   }
//  
//   return Adis16505_Data.Drift_Data.Bit32_Mode.True_Drift;  
//  }
//  else
//  {
//   return 0; 
//  }      
//}



          //二重积分算出；路程
    //         this->Adis16505_Data.X_Axis.Distance+=this->Adis16505_Data.X_Axis.LineVel*Calculus_Time;//每隔10ms积分一次
    //         this->Adis16505_Data.Y_Axis.Distance+=this->Adis16505_Data.Y_Axis.LineVel*Calculus_Time;//每隔10ms积分一次
//             printf("Orginal AngelVel: %f    Filter Angelvel: %f   Threshold Angelvel: %f   Real Angle: %f   hour%d min%d sec%d msec%d \r\n",ZZ,Adis16505_Data.original_.anglevel,\
//             this->Adis16505_Data.Z_Axis.AngelVel,this->Adis16505_Data.Z_Axis.Angle,ImuTime.hour, \
//             ImuTime.min, ImuTime.sec,ImuTime.msec);
//             printf(" ms%d \r\n",ImuTime.msec); 
//             printf("Orginal data: %f  Filter Angelvel: %f\r\n"  ,  ((float)Data_32Bit[3]), Adis16505_Data.original_.anglevel);
//             ReportData(0x52,short(Movaverage.mv_z.anglevel*1000/0.061f),short(LowpassFilter.Z_Axis.AngelVel*1000/0.061f),short(ZZ*1000/0.061f),0) ;
//             ReportData(0x53,short(Movaverage.mv_z.angl*1000/0.55f),short(LowpassFilter.Z_Axis.Angle*1000/0.55f),short(this->Adis16505_Data.Z_Axis.Angle*1000/0.55f),0) ; 
//             printf("\r\n ms%d \r\n",ImuTime.msec);    
           //  ReportData(0x52,short(0x0001/0.061f),0x0002,0x0003,0) ;   
           //  ReportData(0x53,(0x0001)*100/0.55f,0x0003,0x0003,0) ;     
//             ReportData(0x53,short(Movaverage.mv_z.angl*1000/0.55f),short(LowpassFilter.Z_Axis.Angle*1000/0.55f),short(this->Adis16505_Data.Z_Axis.Angle*1000/0.55f),0) ;
